#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/common/distances.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fstream>
#include <sstream>

#include <arpa/inet.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <signal.h>
#include <sys/socket.h>
#include <math.h>
#include <pthread.h>

#include "check.h"

using namespace pcl;
using namespace std;

namespace {
static const size_t buffer_length = 256;
static const float scaling_factor = 1000;
}

class SimpleKinectViewer {
public:
  SimpleKinectViewer(const Eigen::Matrix4f& transform, const string& serial_number)
    : transform_(transform), serial_number_(serial_number), viewer_("PCL Kinect Viewer"),
      set_(false), has_data_(false) {}
  
  void cloud_cb_ (const PointCloud<PointXYZRGBA>::ConstPtr &cloud) {
    if (cloud_mutex_.try_lock()) {
      current_cloud_ = cloud;
      set_ = true;
      cloud_mutex_.unlock();
    }
  }

  void run() {
    Grabber* interface = new OpenNIGrabber();

    boost::function<void (const PointCloud<PointXYZRGBA>::ConstPtr&)> f =
      boost::bind(&SimpleKinectViewer::cloud_cb_, this, _1);
    pthread_t thread_id;
    pthread_create(&thread_id, NULL, &bootstrapReadInputThread, this);

    interface->registerCallback(f);
    interface->start();
    viewer_.setBackgroundColor (0.1, 0.1, 0.1);
    viewer_.addCoordinateSystem (0.1);
    while (!viewer_.wasStopped()) {
      if (set_) {
	boost::mutex::scoped_lock lock(cloud_mutex_);
	if (!viewer_.updatePointCloud(current_cloud_, "first cloud")) {
	  viewer_.addPointCloud(current_cloud_, "first cloud");
	  viewer_.resetCameraViewpoint("first cloud");
	}
	if (has_data_) {
	  Eigen::Vector4f face_loc, gaze_loc, gaze_line;
	  {
	    boost::mutex::scoped_lock lock(data_mutex_);
	    viewer_.removeShape("gaze line");
	    viewer_.addLine(face_loc_, gaze_loc_, 255, 0, 0, "gaze line");
	    face_loc(0) = face_loc_.x;
	    face_loc(1) = face_loc_.y;
	    face_loc(2) = face_loc_.z;
	    face_loc(3) = 0;
	    gaze_loc(0) = gaze_loc_.x;
	    gaze_loc(1) = gaze_loc_.y;
	    gaze_loc(2) = gaze_loc_.z;
	    gaze_loc(3) = 0;
	  }
	  gaze_line = gaze_loc - face_loc;
	  segment(current_cloud_, face_loc, gaze_line);
	}
      }
      viewer_.spinOnce(10);
    }
    interface->stop();
  }

  static inline void* bootstrapReadInputThread(void* this_ptr) {
    return reinterpret_cast<SimpleKinectViewer*>(this_ptr)->readInputThread(NULL);
  }

  int getServerSocket() {
    // Create a server socket.
    int serverfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    CHECK_GE(serverfd, 0);

    // Explicitly ignore SIGPIPE.
    struct sigaction act;
    sigemptyset(&act.sa_mask);
    act.sa_handler = SIG_IGN;
    act.sa_flags = 0;
    sigaction(SIGPIPE, &act, NULL);

    // Bind the socket to a port and begin listening for connections.
    sockaddr_in sin;
    sin.sin_family = AF_INET;
    sin.sin_port = htons(1338);
    sin.sin_addr.s_addr = INADDR_ANY;
    CHECK_GE(bind(serverfd, reinterpret_cast<sockaddr*>(&sin), sizeof(sin)), 0);
    CHECK_GE(listen(serverfd, 1), 0);
    return serverfd;
  }

  int acceptClientSocket(int serverfd) {
    sockaddr_in client_addr;
    socklen_t size = sizeof(client_addr);
    int clientfd = accept(serverfd,
			  reinterpret_cast<sockaddr*>(&client_addr),
			  &size);
    CHECK_GE(clientfd, 0);
    return clientfd;
  }

  void readInput(int socketfd) {
    boost::scoped_array<char> buffer(new char[buffer_length]);
    size_t pos = 0;
    float x1, y1, z1, x2, y2, z2;
    while (!viewer_.wasStopped()) {
      while (string(buffer.get(), pos).find('\n') == string::npos) {
	if (pos >= buffer_length) {
	  cerr << "Buffer is full but no linebreaks were read. Closing connection\n";
	  close(socketfd);
	} 
	int bytes_read = read(socketfd, &buffer[pos], buffer_length - pos);
	if (bytes_read <= 0) {
	  cerr << "Error " << bytes_read << " while reading. Closing connection\n";
	  close(socketfd);
	  return;
	}
	pos += bytes_read;
      }
      string line(buffer.get(), pos);
      size_t last_linebreak_pos = line.rfind('\n');
      CHECK_NE(last_linebreak_pos, string::npos);
      size_t prev_linebreak_pos = string(buffer.get(), last_linebreak_pos).rfind('\n');
      if (prev_linebreak_pos == string::npos) {
	prev_linebreak_pos = 0;
      } else {
	++prev_linebreak_pos;
      }
      CHECK_LT(prev_linebreak_pos, last_linebreak_pos);
      line = line.substr(prev_linebreak_pos, last_linebreak_pos - prev_linebreak_pos);
      CHECK_GE(pos, last_linebreak_pos + 1);
      memmove(buffer.get(), &buffer[last_linebreak_pos + 1], pos - last_linebreak_pos - 1);
      pos -= last_linebreak_pos + 1;

      stringstream ss(line, ios_base::in);
      ss >> x1 >> y1 >> z1 >> x2 >> y2 >> z2;
      cout << "Read " << x1 << " " << y1 << " " << z1 << " " << x2 << " " << y2 << " " << z2 << endl;
      x1 /= scaling_factor;
      y1 /= scaling_factor;
      z1 /= scaling_factor;
      x2 /= scaling_factor;
      y2 /= scaling_factor;
      z2 /= scaling_factor;
      Eigen::Vector4f pos1(x1, y1, z1, 1);
      Eigen::Vector4f pos2(x2, y2, z2, 1);
      Eigen::Vector4f result_pos1 = transform_ * pos1;
      Eigen::Vector4f result_pos2 = transform_ * pos2;
      {
	boost::mutex::scoped_lock lock(data_mutex_);
	face_loc_.x = result_pos1(0);
	face_loc_.y = result_pos1(1);
	face_loc_.z = result_pos1(2);
	gaze_loc_.x = result_pos2(0);
	gaze_loc_.y = result_pos2(1);
	gaze_loc_.z = result_pos2(2);
      }
      has_data_ = true;
    }
  }

  void* readInputThread(void* unnused) {
    int serverfd = getServerSocket();
    
    while (!viewer_.wasStopped()) {
      int socketfd = acceptClientSocket(serverfd);
      readInput(socketfd);
    }
    return NULL;
  }

  PointCloud<PointXYZRGBA>::Ptr filterCloud(const PointCloud<PointXYZRGBA>::ConstPtr& cloud) {
    PointCloud<PointXYZRGBA>::Ptr pass_cloud(new PointCloud<PointXYZRGBA>);
    PointCloud<PointXYZRGBA>::Ptr pass_cloud2(new PointCloud<PointXYZRGBA>);
    PassThrough<PointXYZRGBA> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(1.0, 1.7);
    pass.filter(*pass_cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(-0.5, 0.5);
    pass.setInputCloud(pass_cloud);
    pass.filter(*pass_cloud2);
    
    PointCloud<PointXYZRGBA>::Ptr voxel_cloud(new PointCloud<PointXYZRGBA>);
    VoxelGrid<PointXYZRGBA> vg;
    vg.setInputCloud(pass_cloud2);
    vg.setLeafSize(0.005f, 0.005f, 0.005f);
    vg.filter(*voxel_cloud);
    return voxel_cloud;
  }

  void segment(const PointCloud<PointXYZRGBA>::ConstPtr& cloud,
	       Eigen::Vector4f face_loc, Eigen::Vector4f gaze_line) {
    PointCloud<PointXYZRGBA>::Ptr cloud_filtered = filterCloud(cloud);

    // Create the segmentation object for the planar model and set all the parameters
    SACSegmentation<PointXYZRGBA> seg;
    PointIndices::Ptr inliers (new PointIndices);
    ModelCoefficients::Ptr coefficients (new ModelCoefficients);
    PointCloud<PointXYZRGBA>::Ptr
      cloud_plane (new PointCloud<PointXYZRGBA>), cloud_f (new PointCloud<PointXYZRGBA>);
    seg.setOptimizeCoefficients (true);
    seg.setModelType (SACMODEL_PLANE);
    seg.setMethodType (SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (0.02);

    int i=0, nr_points = (int) cloud_filtered->points.size ();
    while (cloud_filtered->points.size () > 0.3 * nr_points) {
      // Segment the largest planar component from the remaining cloud
      seg.setInputCloud (cloud_filtered);
      seg.segment (*inliers, *coefficients);
      if (inliers->indices.size () == 0) {
	cout << "Could not estimate a planar model for the given dataset." << endl;
	break;
      }
      
      // Extract the planar inliers from the input cloud
      ExtractIndices<PointXYZRGBA> extract;
      extract.setInputCloud (cloud_filtered);
      extract.setIndices (inliers);
      extract.setNegative (false);
      
      extract.filter (*cloud_plane);
      
      // Remove the planar inliers, extract the rest
      extract.setNegative (true);
      extract.filter (*cloud_f);
      cloud_filtered = cloud_f;
    }

    // Creating the KdTree object for the search method of the extraction
    search::KdTree<PointXYZRGBA>::Ptr tree (new search::KdTree<PointXYZRGBA>);
    tree->setInputCloud (cloud_filtered);
    
    vector<PointIndices> cluster_indices;
    EuclideanClusterExtraction<PointXYZRGBA> ec;
    ec.setClusterTolerance (0.02); // 2cm
    ec.setMinClusterSize (100);
    ec.setMaxClusterSize (25000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud_filtered);
    ec.extract (cluster_indices);

    vector<PointCloud<PointXYZRGBA>::Ptr> cluster_list;
    double min_dist = -1;
    int min_index = -1;
    int j = 0;
    for (vector<PointIndices>::const_iterator it = cluster_indices.begin();
	 it != cluster_indices.end (); ++it) {
      PointCloud<PointXYZRGBA>::Ptr cloud_cluster (new PointCloud<PointXYZRGBA>);
      double dist = -1;
      for (vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) {
	cloud_cluster->points.push_back (cloud_filtered->points[*pit]);
	// Compute the distance of the point from the gaze line.
	PointXYZRGBA point = cloud_filtered->points[*pit];
	Eigen::Vector4f pt(point.x, point.y, point.z, 0);
	double d = sqrPointToLineDistance(pt, face_loc, gaze_line);
	if (dist == -1 || d < dist) {
	  dist = d;
	}
      }
      cout << dist << " ";
      if (min_dist == -1 || (dist < 0.01 && dist < min_dist)) {
	min_dist = dist;
	min_index = j;
      }
      cloud_cluster->width = cloud_cluster->points.size ();
      cloud_cluster->height = 1;
      cloud_cluster->is_dense = true;
      cluster_list.push_back(cloud_cluster);
      ++j;
    }
    cout << "-- " << cluster_list.size() << " clusters in total" << endl;
    if (min_index != -1) {
      cout << "Cluster " << min_index << " selected." << endl;
      visualization::PointCloudColorHandlerCustom<PointXYZRGBA> colorizer(cluster_list[min_index], 255, 0, 0);
      if (!viewer_.updatePointCloud(cluster_list[min_index], colorizer, "selected cloud")) {
	viewer_.addPointCloud(cluster_list[min_index], colorizer, "selected cloud");
      }
    }
  }

  const Eigen::Matrix4f& transform_;
  const string& serial_number_;
  visualization::PCLVisualizer viewer_;
  PointCloud<PointXYZRGBA>::ConstPtr current_cloud_;
  boost::mutex cloud_mutex_, data_mutex_;
  bool set_;
  PointXYZ face_loc_, gaze_loc_;
  bool has_data_;
};

int main(int argc, char* argv[]) {
  if (argc < 3) {
    cout << "Usage: " << argv[0] << " [path-to-matrix-file] [kinect device id]\n";
    return 1;
  } else {
    ifstream input(argv[1], ifstream::in);
    Eigen::Matrix4f transform;
    for (int i = 0; i < 4; ++i) {
      input >> transform(i, 0) >> transform(i, 1) >> transform(i, 2) >> transform(i, 3);
    }
    cout << "Transformation matrix is " << endl << transform << endl;
    
    SimpleKinectViewer v(transform, argv[2]);
    v.run();
    return 0;
  }
}
